package Detour

import (
	"math"
)

const (
	H_SCALE = 0.999
)

///DTQueryData////////////////////////////
type DTQueryData struct {
	Status           DTStatus
	LastBestNode     *DTNode
	lastBestNodeCost float64
	StartRef         DTPolyRef
	EndRef           DTPolyRef

	StartPos []float64
	EndPos   []float64

	Filter *DTQueryFilter
}

func (this *DTQueryData) DTCSClear() {
	this.Status = 0
	this.LastBestNode = nil
	this.lastBestNodeCost = 0
	this.StartRef = 0
	this.EndRef = 0
	for i := 0; i < 3; i++ {
		this.StartPos[i] = 0
		this.EndPos[i] = 0
	}
	this.Filter = nil
}

///DTQueryFilter////////////////////////////
type DTQueryFilter struct {
	M_AreaCost     []float64
	M_includeFlags uint16
	M_excludeFlags uint16
}

func NewDTQueryFilter() *DTQueryFilter {
	qf := &DTQueryFilter{}
	qf.M_includeFlags = 0xffff
	qf.M_excludeFlags = 0
	qf.M_AreaCost = make([]float64, DT_MAX_AREAS)
	for i := 0; i < DT_MAX_AREAS; i++ {
		qf.M_AreaCost[i] = 1.0
	}
	return qf
}

func (this *DTQueryFilter) PassFilter(polyRef DTPolyRef, tile *DTMeshTile, poly *DTPoly) bool {
	return (poly.Flags&this.M_includeFlags) != 0 && (poly.Flags&this.M_excludeFlags) == 0
}

func (this *DTQueryFilter) GetCost(pa, pb []float64, prevRef DTPolyRef, prevTile *DTMeshTile, prevPoly *DTPoly,
	curRef DTPolyRef, curTile *DTMeshTile, curPoly *DTPoly, nextRef DTPolyRef, nextTile *DTMeshTile, nextPoly *DTPoly) float64 {
	return DTVdist(pa, pb) * this.M_AreaCost[curPoly.getArea()]
}

///DTNavMeshQuery////////////////////////////
type DTNavMeshQuery struct {
	M_Nav          *DTNavMesh
	M_Query        *DTQueryData
	M_TinyNodePool *DTNodePool
	M_NodePool     *DTNodePool
	M_OpenList     *DTNodeQueue
}

func NewDTNavMeshQuery() *DTNavMeshQuery {
	dq := &DTNavMeshQuery{}
	return dq
}

func (this *DTNavMeshQuery) Init(nav *DTNavMesh, maxNodes int) DTStatus {
	this.M_Nav = nav
	if this.M_NodePool == nil || this.M_NodePool.getMaxNodes() < maxNodes {
		if this.M_NodePool != nil {
			this.M_NodePool = nil
		}
		this.M_NodePool = NewDTNodePool(maxNodes, int(DTNextPow2(uint(maxNodes)/4)))
		if this.M_NodePool == nil {
			return DT_FAILURE | DT_OUT_OF_MEMORY
		}
	} else {
		this.M_NodePool.Clear()
	}
	if this.M_TinyNodePool == nil {
		this.M_TinyNodePool = NewDTNodePool(64, 32)
		if this.M_TinyNodePool == nil {
			return DT_FAILURE | DT_OUT_OF_MEMORY
		}
	} else {
		this.M_TinyNodePool.Clear()
	}

	if this.M_OpenList == nil || this.M_OpenList.GetCapacity() < maxNodes {
		if this.M_OpenList != nil {
			this.M_OpenList = nil
		}
		this.M_OpenList = NewDTNodeQueue(maxNodes)
		if this.M_OpenList == nil {
			return DT_FAILURE | DT_OUT_OF_MEMORY
		}
	} else {
		this.M_OpenList.Clear()
	}
	return DT_SUCCESS
}

func (this *DTNavMeshQuery) QueryPolygons(center []float64, extents []float64, filter *DTQueryFilter,
	polys []DTPolyRef, polyCount *int, maxPolys int) DTStatus {
	bmin := make([]float64, 3)
	bmax := make([]float64, 3)
	dtVsub(bmin, center, extents)
	DtVadd(bmax, center, extents)
	minx, miny, maxx, maxy := 0, 0, 0, 0
	this.M_Nav.CalcTileLoc(bmin, &minx, &miny)
	this.M_Nav.CalcTileLoc(bmax, &maxx, &maxy)

	MAX_NEIS := 32
	neis := make([]DTMeshTile, MAX_NEIS)
	n := 0
	for y := miny; y <= maxy; y++ {
		for x := minx; x <= maxx; x++ {
			nneis := this.M_Nav.getTilesAt(x, y, neis, MAX_NEIS)
			for j := 0; j < nneis; j++ {
				n += this.QueryPolygonsInTile(&neis[j], bmin, bmax, filter, polys, n, maxPolys-n)
				if n >= maxPolys {
					*polyCount = n
					return DT_SUCCESS | DT_BUFFER_TOO_SMALL
				}
			}
		}
	}
	*polyCount = n
	return DT_SUCCESS
}

func (this *DTNavMeshQuery) QueryPolygonsInTile(tile *DTMeshTile, qmin, qmax []float64,
	filter *DTQueryFilter, polys []DTPolyRef, polyStart, maxPolys int) int {
	if tile.BVTree != nil {
		node := tile.BVTree[0]
		endIndex := tile.Header.BvNodeCount
		tbmin := tile.Header.Bmin
		tbmax := tile.Header.Bmax
		qfac := tile.Header.BvQuantFactor
		// Calculate quantized box
		bmin := make([]uint16, 3)
		bmax := make([]uint16, 3)
		// dtClamp query box to world box.
		minx := DTClampFloat(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]
		miny := DTClampFloat(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]
		minz := DTClampFloat(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]
		maxx := DTClampFloat(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]
		maxy := DTClampFloat(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]
		maxz := DTClampFloat(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]
		// Quantize
		bmin[0] = uint16(int(qfac*minx) & 0xfffe)
		bmin[1] = uint16(int(qfac*miny) & 0xfffe)
		bmin[2] = uint16(int(qfac*minz) & 0xfffe)
		bmax[0] = uint16(int(qfac*maxx+1) | 1)
		bmax[1] = uint16(int(qfac*maxy+1) | 1)
		bmax[2] = uint16(int(qfac*maxz+1) | 1)

		// Traverse tree
		polyRefBase := this.M_Nav.GetPolyRefBase(tile)
		n := 0
		nodeIndex := 0
		for nodeIndex < endIndex {
			node = tile.BVTree[nodeIndex]

			overlap := dtOverlapQuantBounds(bmin, bmax, node.Bmin, node.Bmax)
			isLeafNode := node.I >= 0
			if isLeafNode && overlap {
				polyRef := polyRefBase | DTPolyRef(node.I)
				if filter.PassFilter(polyRef, tile, &tile.Polys[node.I]) {
					if n < maxPolys {
						polys[polyStart+n] = polyRef
						n++
					}
				}
			}
			if overlap || isLeafNode {
				nodeIndex++
			} else {
				escapeIndex := -node.I
				nodeIndex += escapeIndex
			}
		}
		return n
	} else {
		bmin := make([]float64, 3)
		bmax := make([]float64, 3)
		n := 0
		polyRefBase := this.M_Nav.GetPolyRefBase(tile)
		for i := 0; i < tile.Header.PolyCount; i++ {
			p := tile.Polys[i]
			if p.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
				continue
			}
			polyRef := polyRefBase | DTPolyRef(i)
			if !filter.PassFilter(polyRef, tile, &p) {
				continue
			}
			vStart := p.Verts[0] * 3
			dtVcopy4(bmin, 0, tile.Verts, int(vStart))
			dtVcopy4(bmax, 0, tile.Verts, int(vStart))
			for j := 0; j < int(p.VertCount); j++ {
				vStart = p.Verts[j] * 3
				DTVminFloat4(bmin, 0, tile.Verts, int(vStart))
				DTVMaxFloat64(bmax, 0, tile.Verts, int(vStart))
			}
			if dtOverlapBounds(qmin, qmax, bmin, bmax) {
				if n < maxPolys {
					polys[polyStart+n] = polyRef
					n++
				}
			}
		}
		return n
	}
}

func (this *DTNavMeshQuery) FindNearestPoly(center, extents []float64, filter *DTQueryFilter,
	nearestRef *DTPolyRef, nearestPt []float64) DTStatus {
	*nearestRef = 0
	polys := make([]DTPolyRef, 128)
	polyCount := 0
	if DTStatusFailed(this.QueryPolygons(center, extents, filter, polys, &polyCount, 128)) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	nearest := 0
	nearestDistanceSqr := math.MaxFloat64
	for i := 0; i < polyCount; i++ {
		polyRef := polys[i]
		closestPtPoly := make([]float64, 3)
		diff := make([]float64, 3)
		posOverPoly := false
		var d float64
		this.ClosestPointOnPoly(polyRef, center, closestPtPoly, &posOverPoly)
		dtVsub(diff, center, closestPtPoly)
		if posOverPoly {
			var tile *DTMeshTile
			tile, _ = this.M_Nav.getTileAndPolyByRefUnsafe3(polys[i])
			d = math.Abs(diff[1]) - tile.Header.WalkableClimb
			if d > 0 {
				d = d * d
			} else {
				d = 0
			}
		} else {
			d = dtVlenSqr(diff)
		}

		if d < nearestDistanceSqr {
			DTVcopy(nearestPt, closestPtPoly)
			nearestDistanceSqr = d
			nearest = int(polyRef)
		}
	}

	*nearestRef = DTPolyRef(nearest)
	return DT_SUCCESS
}

func (this *DTNavMeshQuery) ClosestPointOnPoly(polyRef DTPolyRef, pos, closest []float64, posOverPoly *bool) DTStatus {

	var ip uint
	ret, tile, poly := this.M_Nav.getTileAndPolyByRef(polyRef, &ip)
	if DTStatusFailed(ret) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	if tile == nil {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	if poly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
		v0Start := poly.Verts[0] * 3
		v1Start := poly.Verts[1] * 3
		d0 := DTVdist4(pos, 0, tile.Verts, int(v0Start))
		d1 := DTVdist4(pos, 0, tile.Verts, int(v1Start))
		u := d0 / (d0 + d1)
		DTVlerp7(closest, 0, tile.Verts, int(v0Start), tile.Verts, int(v1Start), u)
		*posOverPoly = false
		return DT_SUCCESS
	}
	pd := tile.DetailMeshes[ip]
	verts := make([]float64, DT_VERTS_PER_POLYGON*3)
	edged := make([]float64, DT_VERTS_PER_POLYGON)
	edget := make([]float64, DT_VERTS_PER_POLYGON)
	nv := int(poly.VertCount)
	for i := 0; i < nv; i++ {
		dtVcopy4(verts, i*3, tile.Verts, int(poly.Verts[i]*3))
	}
	DTVcopy(closest, pos)
	if !dtDistancePtPolyEdgesSqr(pos, 0, verts, nv, edged, edget) {
		dmin := math.MaxFloat64
		imin := -1
		for i := 0; i < nv; i++ {
			if edged[i] < dmin {
				dmin = edged[i]
				imin = i
			}
		}
		vaStart := imin * 3
		vbStart := ((imin + 1) % nv) * 3
		DTVlerp7(closest, 0, verts, vaStart, verts, vbStart, edget[imin])
		*posOverPoly = false
	} else {
		*posOverPoly = true
	}
	for j := 0; j < int(pd.TriCount); j++ {
		tStart := (int(pd.TriBase) + j) * 4
		vStarts := make([]int, 3)
		vSrc := make([][]float64, 3)
		for k := 0; k < 3; k++ {
			tk := tile.DetailTris[tStart+k]
			vCount := poly.VertCount
			if tk < vCount {
				vStarts[k] = int(poly.Verts[tk] * 3)
				vSrc[k] = tile.Verts
			} else {
				vStarts[k] = (int(pd.VertBase) + int(tk-vCount)) * 3
				vSrc[k] = tile.DetailVerts
			}
		}

		h := 0.0
		if dtClosestHeightPointTriangle(pos, 0, vSrc[0], vStarts[0], vSrc[1], vStarts[1], vSrc[2], vStarts[2], &h) {
			closest[1] = h

			break
		}
	}
	return DT_SUCCESS
}

func (this *DTNavMeshQuery) FindPath(startRef, endRef DTPolyRef, startPos, endPos []float64,
	filter *DTQueryFilter, path []DTPolyRef, pathCount *int, maxPath int) DTStatus {
	*pathCount = 0
	if startRef == 0 || endRef == 0 {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	if maxPath == 0 {
		return DT_FAILURE | DT_INVALID_PARAM

	}

	if !this.M_Nav.IsValidPolyRef(startRef) || !this.M_Nav.IsValidPolyRef(endRef) {
		return DT_FAILURE | DT_INVALID_PARAM
	}

	if startRef == endRef {
		path[0] = startRef
		*pathCount = 1
		return DT_SUCCESS
	}
	this.M_NodePool.Clear()
	this.M_OpenList.Clear()

	startNode := this.M_NodePool.GetNode(startRef)
	DTVcopy(startNode.Pos, startPos)
	startNode.Pidx = 0
	startNode.Cost = 0
	startNode.Total = DTVdist(startPos, endPos) * H_SCALE
	startNode.Id = startRef
	startNode.Flags = DT_NODE_OPEN
	this.M_OpenList.push(startNode)

	lastBestNode := startNode
	lastBestNodeCost := startNode.Total
	var status DTStatus = DT_SUCCESS
	for !this.M_OpenList.Empty() {
		bestNode := this.M_OpenList.pop()
		bestNode.Flags &= ^DT_NODE_OPEN
		bestNode.Flags |= DT_NODE_CLOSED

		if bestNode.Id == endRef {
			lastBestNode = bestNode
			break
		}
		bestRef := bestNode.Id
		var bestTile *DTMeshTile
		var bestPoly *DTPoly
		bestTile, bestPoly = this.M_Nav.getTileAndPolyByRefUnsafe3(bestRef)
		var parentRef DTPolyRef = 0
		var parentTile *DTMeshTile
		var parentPoly *DTPoly
		if bestNode.Pidx != 0 {
			parentRef = this.M_NodePool.GetNodeAtIdx(bestNode.Pidx).Id
		}
		if parentRef != 0 {
			parentTile, parentPoly = this.M_Nav.getTileAndPolyByRefUnsafe3(parentRef)
		}

		for i := bestPoly.FirstLink; i != DT_NULL_LINK; i = bestTile.Links[i].Next {

			neighbourRef := bestTile.Links[i].polyRef
			if neighbourRef == 0 || neighbourRef == parentRef {
				continue
			}
			var neighbourTile *DTMeshTile
			var neighbourPoly *DTPoly
			neighbourTile, neighbourPoly = this.M_Nav.getTileAndPolyByRefUnsafe3(neighbourRef)
			if !filter.PassFilter(neighbourRef, neighbourTile, neighbourPoly) {

				continue
			}

			neighbourNode := this.M_NodePool.GetNode(neighbourRef)

			if neighbourNode == nil {
				status |= DT_OUT_OF_NODES

				continue
			}
			// If the node is visited the first time, calculate node position.
			if neighbourNode.Flags == 0 {
				this.GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, neighbourNode.Pos)
			}
			var Cost, heuristic float64
			if neighbourRef == endRef {
				//Cost
				curCost := filter.GetCost(bestNode.Pos, neighbourNode.Pos, parentRef, parentTile, parentPoly,
					bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly)
				endCost := filter.GetCost(neighbourNode.Pos, endPos, bestRef, bestTile, bestPoly,
					neighbourRef, neighbourTile, neighbourPoly, 0, nil, nil)
				Cost = bestNode.Cost + curCost + endCost
				heuristic = 0
			} else {
				curCost := filter.GetCost(bestNode.Pos, neighbourNode.Pos, parentRef, parentTile, parentPoly,
					bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly)
				Cost = bestNode.Cost + curCost
				heuristic = DTVdist(neighbourNode.Pos, endPos) * H_SCALE
			}
			total := Cost + heuristic
			if (neighbourNode.Flags&DT_NODE_OPEN) != 0 && total >= neighbourNode.Total {
				continue
			}
			if (neighbourNode.Flags&DT_NODE_CLOSED) != 0 && total >= neighbourNode.Total {
				continue
			}
			neighbourNode.Pidx = this.M_NodePool.GetNodeIdx(bestNode)
			neighbourNode.Id = neighbourRef
			neighbourNode.Flags = neighbourNode.Flags & ^DT_NODE_CLOSED
			neighbourNode.Cost = Cost
			neighbourNode.Total = total
			if (neighbourNode.Flags & DT_NODE_OPEN) != 0 {
				this.M_OpenList.modify(neighbourNode)
			} else {
				neighbourNode.Flags |= DT_NODE_OPEN
				this.M_OpenList.push(neighbourNode)
			}
			if heuristic < lastBestNodeCost {
				lastBestNodeCost = heuristic
				lastBestNode = neighbourNode
			}
		}
	}

	if lastBestNode.Id != endRef {
		status |= DT_PARTIAL_RESULT
	}

	var prev, node *DTNode
	node = lastBestNode
	for node != nil {
		next := this.M_NodePool.GetNodeAtIdx(node.Pidx)
		node.Pidx = this.M_NodePool.GetNodeIdx(prev)
		prev = node
		node = next
	}
	node = prev
	n := 0
	for node != nil {
		path[n] = node.Id
		n++
		if n >= maxPath {
			status |= DT_BUFFER_TOO_SMALL
			break
		}
		node = this.M_NodePool.GetNodeAtIdx(node.Pidx)
	}
	*pathCount = n
	return status
}

func (this *DTNavMeshQuery) GetEdgeMidPoint(from DTPolyRef, fromPoly *DTPoly, fromTile *DTMeshTile,
	to DTPolyRef, toPoly *DTPoly, toTile *DTMeshTile, mid []float64) DTStatus {
	left := make([]float64, 3)
	right := make([]float64, 3)
	if DTStatusFailed(this.GetPortalPoints8(from, fromPoly, fromTile, to, toPoly, toTile, left, right)) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	mid[0] = (left[0] + right[0]) * 0.5
	mid[1] = (left[1] + right[1]) * 0.5
	mid[2] = (left[2] + right[2]) * 0.5
	return DT_SUCCESS
}

func (this *DTNavMeshQuery) GetPortalPoints(from DTPolyRef, to DTPolyRef, left,
	right []float64, fromType, toType *byte) DTStatus {
	var fromTile *DTMeshTile
	var fromPoly *DTPoly
	if DTStatusFailed(this.M_Nav.getTileAndPolyByRef3(from, &fromTile, &fromPoly)) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	*fromType = fromPoly.getType()
	var ToTile *DTMeshTile
	var toPoly *DTPoly

	if DTStatusFailed(this.M_Nav.getTileAndPolyByRef3(to, &ToTile, &toPoly)) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	*toType = toPoly.getType()
	return this.GetPortalPoints8(from, fromPoly, fromTile, to, toPoly, ToTile, left, right)
}

func (this *DTNavMeshQuery) ClosestPointOnPolyBoundary(polyRef DTPolyRef, pos, closest []float64) DTStatus {
	var tile *DTMeshTile
	var poly *DTPoly

	if DTStatusFailed(this.M_Nav.getTileAndPolyByRef3(polyRef, &tile, &poly)) {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	verts := make([]float64, DT_VERTS_PER_POLYGON*3)
	edged := make([]float64, DT_VERTS_PER_POLYGON)
	edget := make([]float64, DT_VERTS_PER_POLYGON)
	nv := 0

	for i := 0; i < int(poly.VertCount); i++ {
		dtVcopy4(verts, nv*3, tile.Verts, int(poly.Verts[i]*3))
		nv++
	}

	inside := dtDistancePtPolyEdgesSqr(pos, 0, verts, nv, edged, edget)
	if inside {
		DTVcopy(closest, pos)
	} else {
		dmin := math.MaxFloat64
		imin := -1
		for i := 0; i < nv; i++ {
			if edged[i] < dmin {
				dmin = edged[i]
				imin = i
			}
		}
		vaStart := imin * 3
		vbStart := ((imin + 1) % nv) * 3
		DTVlerp7(closest, 0, verts, vaStart, verts, vbStart, edget[imin])
	}
	return DT_SUCCESS
}

func (this *DTNavMeshQuery) GetPortalPoints8(from DTPolyRef, fromPoly *DTPoly, fromTile *DTMeshTile, to DTPolyRef,
	toPoly *DTPoly, toTile *DTMeshTile, left, right []float64) DTStatus {
	var link *DTLink
	for i := fromPoly.FirstLink; i != DT_NULL_LINK; i = fromTile.Links[i].Next {
		if fromTile.Links[i].polyRef == to {
			link = &fromTile.Links[i]
			break
		}
	}

	if link == nil {
		return DT_FAILURE | DT_INVALID_PARAM
	}
	// Handle off-mesh connections.
	if fromPoly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
		for i := fromPoly.FirstLink; i != DT_NULL_LINK; i = fromTile.Links[i].Next {
			if fromTile.Links[i].polyRef == to {
				v := fromTile.Links[i].Edge
				dtVcopy4(left, 0, fromTile.Verts, int(fromPoly.Verts[v]*3))
				dtVcopy4(right, 0, fromTile.Verts, int(fromPoly.Verts[v]*3))
				return DT_SUCCESS
			}
		}
		return DT_FAILURE | DT_INVALID_PARAM
	}

	if toPoly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION {
		for i := toPoly.FirstLink; i != DT_NULL_LINK; i = toTile.Links[i].Next {
			if toTile.Links[i].polyRef == from {
				v := toTile.Links[i].Edge
				dtVcopy4(left, 0, toTile.Verts, int(toPoly.Verts[v]*3))
				dtVcopy4(right, 0, toTile.Verts, int(toPoly.Verts[v]*3))
				return DT_SUCCESS
			}
		}
		return DT_FAILURE | DT_INVALID_PARAM
	}

	v0 := fromPoly.Verts[link.Edge]
	v1 := fromPoly.Verts[int(link.Edge+1)%int(fromPoly.VertCount)]
	dtVcopy4(left, 0, fromTile.Verts, int(v0*3))
	dtVcopy4(right, 0, fromTile.Verts, int(v1*3))
	if link.Side != 0xff {
		if link.Bmin != 0 || link.Bmax != 255 {
			s := 1.0 / 255.0
			tmin := float64(link.Bmin) * s
			tmax := float64(link.Bmax) * s
			DTVlerp7(left, 0, fromTile.Verts, int(v0*3), fromTile.Verts, int(v1*3), tmin)
			DTVlerp7(right, 0, fromTile.Verts, int(v0*3), fromTile.Verts, int(v1*3), tmax)
		}
	}
	return DT_SUCCESS
}
